#include "stm32f4xx_hal.h" // 根据你的STM32型号选择相应的头文件  
  
// PID控制器结构体  
typedef struct {  
    float SetPoint; // 设定值  
    float Proportional; // 比例系数  
    float Integral; // 积分值  
    float Derivative; // 微分值  
    float LastError; // 上一次误差  
    float IntegralLimit; // 积分限幅  
    float Kp; // 比例系数  
    float Ki; // 积分系数  
    float Kd; // 微分系数  
    float Output; // 输出值  
} PID_Controller;  
  
// PID初始化函数  
void PID_Init(PID_Controller* pid, float kp, float ki, float kd, float integralLimit) {  
    pid->Proportional = 0.0;  
    pid->Integral = 0.0;  
    pid->Derivative = 0.0;  
    pid->LastError = 0.0;  
    pid->Kp = kp;  
    pid->Ki = ki;  
    pid->Kd = kd;  
    pid->IntegralLimit = integralLimit;  
}  
  
// PID计算函数  
float PID_Compute(PID_Controller* pid, float measuredValue) {  
    float error = pid->SetPoint - measuredValue;  
  
    pid->Proportional = pid->Kp * error;  
  
    pid->Integral += error;  
  
    // 防止积分溢出  
    if (pid->Integral > pid->IntegralLimit)  
        pid->Integral = pid->IntegralLimit;  
    else if (pid->Integral < -pid->IntegralLimit)  
        pid->Integral = -pid->IntegralLimit;  
  
    pid->Derivative = error - pid->LastError;  
    pid->LastError = error;  
  
    pid->Output = pid->Proportional + pid->Integral * pid->Ki + pid->Derivative * pid->Kd;  
  
    return pid->Output;  
}


// 假设你有一个PWM初始化函数和设置PWM值的函数  
void PWM_Init(void);  
void PWM_SetValue(uint16_t value);  
  
// 电机控制函数  
void Motor_Control(PID_Controller* pid, float speedMeasurement) {  
    float pwmValue = PID_Compute(pid, speedMeasurement);  
    // 根据PWM的范围限制输出值  
    if (pwmValue > 100.0) pwmValue = 100.0;  
    if (pwmValue < 0.0) pwmValue = 0.0;  
      
    // 将PID输出转换为PWM值并设置PWM  
    PWM_SetValue((uint16_t)(pwmValue * 1000.0)); // 假设PWM分辨率为1000  
}

int pid_task(void) {
    HAL_Init(); // 初始化HAL库  
    SystemClock_Config(); // 配置系统时钟，这个函数需要你根据具体硬件实现  
  
    PID_Controller motorPID;  
    PID_Init(&motorPID, 1.0, 0.1, 0.01, 100.0); // 初始化PID参数，这些值需要根据实际情况调整  
    motorPID.SetPoint = 50.0; // 设定速度目标值  
  
    PWM_Init(); // 初始化PWM  
  
    while (1) {  
        float speedMeasurement = GetSpeedMeasurement(); // 获取电机速度测量值，这个函数需要你根据具体硬件实现  
        Motor_Control(&motorPID, speedMeasurement); // 使用PID控制电机速度  
        HAL_Delay(10); // 延时，根据实际需要调整  
    }  
}